#ifdef SYPX_MEMCHECK
#include "mmgr.h"
#endif

#include "Graph.h"
#include "Instances.h"
#include "Level.h"
#include "Wolf.h"
#include <algorithm>
#include "Wolf.h"

namespace SheepDefence
{
	Graph::Graph()
	{
	}

	Graph::~Graph()
	{
		for (unsigned int i = 0; i < nodes.size(); i++)
			delete nodes[i];
	}

	Graph::Graph(int num_nodes, Level* lvl) : age(0), owner(lvl)
	{
		for (int i = 0; i < num_nodes; i++)
			nodes.push_back(new Node());
	}

	void Graph::updateLevel(Level* lvl)
	{
		owner = lvl;
	}

	void Graph::update()
	{
		std::vector<Node*>::iterator i = nodes.begin();

		while (i != nodes.end())
		{
			(*i)->update();
			i++;
		}
	}

	//Problem function, optimise the shit out of this
	SypX::Vec2 Graph::getDest(WolfInstance* wolf, DogInstance* dog, std::vector<SheepInstance*>& sheep, std::vector<ObstacleInstance*>& obstacles)
	{
		SypX::Vec2 result;
		
		if (wolf->level->numSheep == 0)
		{
			result.x = wolf->x;
			result.y = wolf->y;
			return result;
		}

		std::vector<Node*>::iterator i = nodes.begin();

		while (i != nodes.end())
		{
			(*i)->reset();

			std::vector<SheepInstance*>::iterator j = sheep.begin();

			while (j != sheep.end())
			{
				if ((*j)->state != DEAD && (*j)->state != BITTEN && (*j)->state != TARGETED)
					(*i)->addOrigin((*j)->x, (*j)->y, obstacles, wolf, dog);
				j++;
			}

			i++;
		}

		bool change = true;

		while(change)
		{
			change = false;
			i = nodes.begin();
			
			while(i != nodes.end())
			{
				bool temp = (*i)->calculate(wolf, dog);
				change = change || temp;
				i++;
			}
		}

		std::sort(nodes.begin(), nodes.end(), nodeComparator());

		i = nodes.begin();
		
		while(i != nodes.end()) 
		{
			if ((*i)->point != -1 && !Instance::collision(wolf->x, wolf->y, (*i)->x, (*i)->y, obstacles, wolf, dog))
			{
				result.x = (*i)->x;
				result.y = (*i)->y;
				return result;
			}

			i++;
		}

		return getDest(wolf, sheep, obstacles);
	}

	SypX::Vec2 Graph::getDest(WolfInstance* wolf, std::vector<SheepInstance*>& sheep, std::vector<ObstacleInstance*>& obstacles)
	{
		// Method one, enable hungry: If all sheep are protected, wolves cannot go for safe kills, they will ignore the dog
		// Method two, disable hungry: Wolves will keep trying to go near but will be repelled

		wolf->changeState(HUNGRY);

		SypX::Vec2 result;
		
		if (wolf->level->numSheep == 0)
		{
			result.x = wolf->x;
			result.y = wolf->y;
			return result;
		}

		std::vector<Node*>::iterator i = nodes.begin();

		while (i != nodes.end())
		{
			(*i)->reset();

			std::vector<SheepInstance*>::iterator j = sheep.begin();

			while (j != sheep.end())
			{
				if ((*j)->state != DEAD && (*j)->state != BITTEN && (*j)->state != TARGETED)
					(*i)->addOrigin((*j)->x, (*j)->y, obstacles, wolf->actor->collision);
				j++;
			}

			i++;
		}

		bool change = true;

		while(change)
		{
			change = false;
			i = nodes.begin();
			
			while(i != nodes.end())
			{
				bool temp = (*i)->calculate();
				change = change || temp;
				i++;
			}
		}

		std::sort(nodes.begin(), nodes.end(), nodeComparator());

		i = nodes.begin();
		
		while(i != nodes.end()) 
		{
			if ((*i)->point != -1 && !Instance::collision(wolf->x, wolf->y, (*i)->x, (*i)->y, obstacles, wolf->actor->collision))
			{
				result.x = (*i)->x;
				result.y = (*i)->y;
				return result;
			}

			i++;
		}

		result.x = wolf->x;
		result.y = wolf->y;

		return result;
	}

	SypX::Vec2 Graph::getDest(float start_x, float start_y, float end_x, float end_y, std::vector<ObstacleInstance*>& obstacles, float size)
	{
		std::vector<ObstacleInstance*> blank;

		return getDest(start_x, start_y, end_x, end_y, obstacles, size, blank);
	}

	SypX::Vec2 Graph::getDest(float start_x, float start_y, float end_x, float end_y, std::vector<ObstacleInstance*>& obstacles, float size, std::vector<ObstacleInstance*>& exceptions)
	{
		SypX::Vec2 result;
		
		std::vector<Node*>::iterator i = nodes.begin();

		while (i != nodes.end())
		{
			(*i)->reset();
			(*i)->addOrigin(end_x, end_y, obstacles, size, exceptions);

			i++;
		}

		bool change = true;

		while(change)
		{
			change = false;
			i = nodes.begin();
			
			while(i != nodes.end())
			{
				bool temp = (*i)->calculate();
				change = change || temp;
				i++;
			}
		}

		std::sort(nodes.begin(), nodes.end(), nodeComparator());

		i = nodes.begin();

		while(i != nodes.end()) 
		{
			if (!Instance::collision(start_x, start_y, (*i)->x, (*i)->y, obstacles, size, exceptions))
			{
				result.x = (*i)->x;
				result.y = (*i)->y;
				return result;
			}

			i++;
		}

		result.x = end_x;
		result.y = end_y;
		return result;
	}

	void Graph::setup(std::vector<WolfInstance*>& wolves, DogInstance* dog, std::vector<SheepInstance*>& sheep, std::vector<ObstacleInstance*>& obstacles, float time)
	{
		age += time;

		if (age < 0.1)
			return;
		else
			age = 0;

		if (wolves.size() == 0)
			return;

		WolfInstance* max = wolves[0];

		std::vector<WolfInstance*>::iterator j = wolves.begin();

		while (j != wolves.end())
		{
			if ((*j)->state != DEAD && (*j)->state != BITTEN && (*j)->actor->collision + (*j)->actor->fear > max->actor->collision + (*j)->actor->fear)	// approximate for largest possible collision
				max = (*j);
			j++;
		}

		std::vector<Node*>::iterator i = nodes.begin();

		while (i != nodes.end())
		{
			(*i)->sharedReset();

			std::vector<SheepInstance*>::iterator j = sheep.begin();

			while (j != sheep.end())
			{
				if ((*j)->state != DEAD && (*j)->state != BITTEN && (*j)->state != TARGETED)
					(*i)->addSharedOrigin((*j)->x, (*j)->y, obstacles, max, dog);
				j++;
			}

			i++;
		}

		bool change = true;

		while(change)
		{
			change = false;
			i = nodes.begin();
			
			while(i != nodes.end())
			{
				bool temp = (*i)->calculateShared(max, dog);
				change = change || temp;
				i++;
			}
		}
	}

	SypX::Vec2 Graph::getDest(WolfInstance* wolf, DogInstance* dog, std::vector<ObstacleInstance*>& obstacles)
	{
		SypX::Vec2 result;
		
		if (wolf->level->numSheep == 0)
		{
			result.x = wolf->x;
			result.y = wolf->y;
			return result;
		}

		std::vector<Node*>::iterator i = nodes.begin();
/*			
		while(i != nodes.end())
		{
			(*i)->shared += Instance::getDist(wolf->x - (*i)->x, wolf->y - (*i)->y) / 2;
			i++;
		}
*/		
		std::sort(nodes.begin(), nodes.end(), nodeSharedComparator());

		bool returned = false;

		i = nodes.begin();
		
		while(i != nodes.end()) 
		{
			if ((*i)->shared != -1 && !Instance::collision(wolf->x, wolf->y, (*i)->x, (*i)->y, obstacles, wolf, dog))
			{
				result.x = (*i)->x;
				result.y = (*i)->y;
				returned = true;
				break;
			}

			i++;
		}
/*
		i = nodes.begin();
			
		while(i != nodes.end())
		{
			(*i)->shared -= Instance::getDist(wolf->x - (*i)->x, wolf->y - (*i)->y) / 2;
			i++;
		}
*/
		if (returned)
			return result;
		else
			return getDest(wolf, obstacles);
	}

	SypX::Vec2 Graph::getDest(WolfInstance* wolf, std::vector<ObstacleInstance*>& obstacles)
	{
		wolf->changeState(HUNGRY);
		
		SypX::Vec2 result;
		
		if (wolf->level->numSheep == 0)
		{
			result.x = wolf->x;
			result.y = wolf->y;
			return result;
		}

		std::vector<Node*>::iterator i = nodes.begin();
/*			
		while(i != nodes.end())
		{
			(*i)->sharedHungry += Instance::getDist(wolf->x - (*i)->x, wolf->y - (*i)->y) / 2;
			i++;
		}
*/		
		std::sort(nodes.begin(), nodes.end(), nodeSharedHungryComparator());

		bool returned = false;

		i = nodes.begin();
		
		while(i != nodes.end()) 
		{
			if ((*i)->sharedHungry != -1 && !Instance::collision(wolf->x, wolf->y, (*i)->x, (*i)->y, obstacles, wolf->actor->collision))
			{
				result.x = (*i)->x;
				result.y = (*i)->y;
				returned = true;
				break;
			}

			i++;
		}
/*
		i = nodes.begin();
			
		while(i != nodes.end())
		{
			(*i)->sharedHungry -= Instance::getDist(wolf->x - (*i)->x, wolf->y - (*i)->y) / 2;
			i++;
		}
*/
		if (returned)
			return result;

		result.x = wolf->x + rand() % 30 - 15;
		result.y = wolf->y + rand() % 30 - 15;

		wolf->changeState(STUCK);

		return result;
	}
}